#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/force_control.h"
#include "dyna/jaka_dyn_intf.h"
#include "axis/axis.h"

#define TEST_COUNT 10

using namespace axisgroup;

class MockAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
};

class FCTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        for (int i = 0; i < 7; i++) { EXPECT_CALL(joint[i], get_id()).WillRepeatedly(testing::Return(i)); }
    }
    virtual void TearDown() override {}

    MockAxis joint[7];
    pid_t pid_;
    int shm_id_ = -1;
};

#define RUN_CYCLE_TIMES 1000

TEST_F(FCTest, SERVO_MOVE)
{
    auto grp_ = std::make_shared<Robot>();
    grp_->reset();
    ZucPose cmd_pose = {100, 200, 300, 10, 20, 30};
    grp_->cmd_rst_.pose = cmd_pose;

    auto fc_ = std::make_shared<ForceControlIntf>(grp_.get());
    ASSERT_TRUE(fc_->force_sensor_.sensor_shm_ != nullptr);
    ASSERT_FALSE(fc_->compliant_enable_state_);

    // 开启力控
    fc_->enable_force_control(cmd_pose);
    ZucPose desired_pose = cmd_pose;
    ZucPose revised_pose;
    int ret = fc_->admittanceRevise(desired_pose, grp_->cmd_rst_, desired_pose);
    EXPECT_EQ(ret, -1);  // 传感器未打开
    fc_->force_sensor_.is_opened_ = true;

    // 带反弹拖拽模式
    fc_->fc_config_.admit_fc_frame = FCAxisPara::FRAME_USER;
    fc_->fc_config_.admit_fc_config[0].enableFt = 1;
    fc_->fc_config_.admit_fc_config[0].ftConstant = 0;
    fc_->fc_config_.admit_fc_config[0].ftReboundFK = 1;
    fc_->fc_config_.admit_fc_config[0].ftUser = 100;
    for (int i = 0; i < RUN_CYCLE_TIMES; i++)
    {
        fc_->force_sensor_.update();
        fc_->force_sensor_.raw_data_[0] = 100;
        int ret = fc_->admittanceRevise(desired_pose, grp_->cmd_rst_, revised_pose);
        ASSERT_EQ(ret, 0);
        // printf("Now : %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        revised_pose.tran.x,
        //        revised_pose.tran.y,
        //        revised_pose.tran.z,
        //        revised_pose.a,
        //        revised_pose.b,
        //        revised_pose.c);
        grp_->cmd_rst_.pose = revised_pose;
    }
    EXPECT_NEAR(grp_->cmd_rst_.pose.tran.x, desired_pose.tran.x + 100, 1);
    // 撤去力
    for (int i = 0; i < RUN_CYCLE_TIMES; i++)
    {
        fc_->force_sensor_.update();
        fc_->force_sensor_.raw_data_[0] = 0;
        int ret = fc_->admittanceRevise(desired_pose, grp_->cmd_rst_, revised_pose);
        ASSERT_EQ(ret, 0);
        // printf("Now : %lf, %lf, %lf, %lf, %lf, %lf\n",
        //        revised_pose.tran.x,
        //        revised_pose.tran.y,
        //        revised_pose.tran.z,
        //        revised_pose.a,
        //        revised_pose.b,
        //        revised_pose.c);
        grp_->cmd_rst_.pose = revised_pose;
    }
    EXPECT_NEAR(grp_->cmd_rst_.pose.tran.x, desired_pose.tran.x, 1);
}